#include "HybridSystemControllerRushed.h"
#include "Servo.h"
#include "LiquidCrystal.h"

#define PIN_MOTOR_DRIVE 9
#define SPEED_DRIVE_OFF 0
#define DELAY_DRIVE_OFF 0
#define SPEED_DRIVE_LOW 10
#define DELAY_DRIVE_LOW 100
#define SPEED_DRIVE_HIGH 18
#define DELAY_DRIVE_HIGH 80
#define PIN_SERVO_CLUTCH 10
#define DEGREE_CLUTCH_DOWN 150
#define DEGREE_CLUTCH_UP 180
float SECONDS_FULL_THROTTLE = 3.00;
#define SECONDS_IDLE 0
#define LOAD_WHEEL_MIN_RPM 100
#define BUTTON_DEBOUNCE 90

LiquidCrystal lcd( 12, 11, 5, 4, 3, 7 );

Servo clutch;

struct tach
{
  unsigned long nMillisLast;
  unsigned long nMillisCurrent;
};

struct motor
{
  unsigned int nPos;
  unsigned int nDirection;
};

struct tach tachLoadWheel;

struct motor drive;

void tachInit( struct tach &tach, int nMillisLastInit, int nMillisCurrentInit )
{
  tach.nMillisLast = nMillisLastInit;
  tach.nMillisCurrent = nMillisCurrentInit;
}

void tachISR()
{
  if ( ( millis() - tachLoadWheel.nMillisCurrent ) >= BUTTON_DEBOUNCE )
  {
    tachLoadWheel.nMillisLast = tachLoadWheel.nMillisCurrent;
    tachLoadWheel.nMillisCurrent = millis();
  }
}

int tachGetRpm( struct tach &tach )
{
  unsigned long nDiff = ( tach.nMillisCurrent - tach.nMillisLast );
  return ( 60000 / nDiff );
}

void motorSetPosition( struct motor & motor, int nTarget, int nDelay )
{
  motor.nDirection = ( ( nTarget > motor.nPos ) ? -1 : 1 );

  while ( !( motor.nPos = nTarget ) )
  {
    motor.nPos = motor.nPos + motor.nDirection;
    analogWrite( PIN_MOTOR_DRIVE, motor.nPos );
    delay( nDelay );
  }
}

void motorInit( struct motor & motor, int nPos, int nDirection )
{
  motor.nPos = nPos;
  motor.nDirection = nDirection;
}

void setup()
{
  lcd.begin( 16, 2 );
  lcd.print( "Initializing..." );

  attachInterrupt( 0, tachISR, FALLING );
  digitalWrite( 2, HIGH );

  tachInit( tachLoadWheel, 0, 0 );

  motorInit( drive, 0, 0 );

  pinMode( PIN_MOTOR_DRIVE, OUTPUT );
  clutch.attach( PIN_SERVO_CLUTCH );

  clutch.write( DEGREE_CLUTCH_UP );

  delay( 5000 );
  lcd.clear();
}

void loop()
{

  lcd.clear();
  lcd.print( "up and slow" );
  clutch.write( DEGREE_CLUTCH_UP );
  motorSetPosition( drive, SPEED_DRIVE_LOW, DELAY_DRIVE_LOW );
  delay( 1000 );

  lcd.clear();
  lcd.print( "down and slow" );
  clutch.write( DEGREE_CLUTCH_DOWN );
  delay(1500);

  lcd.clear();
  lcd.print( "keep idle" );
  delay( SECONDS_IDLE * 1000 );

  lcd.clear();
  lcd.print( "down and high" );
  motorSetPosition( drive, SPEED_DRIVE_HIGH, DELAY_DRIVE_HIGH );

  lcd.clear();
  lcd.print( "keep high" );
  delay( SECONDS_FULL_THROTTLE * 1000 );

  lcd.clear();
  lcd.print( "up and off" );
  motorSetPosition( drive, SPEED_DRIVE_OFF, DELAY_DRIVE_OFF );

  lcd.clear();
  lcd.print( "wait for slow" );
  delay( 1500 );
  while ( tachGetRpm( tachLoadWheel ) > LOAD_WHEEL_MIN_RPM )
  {
    if ( millis() % 10 == 0 )
    {
      lcd.clear();
      lcd.print( "rpm: " );
      lcd.print( tachGetRpm( tachLoadWheel ) );
    }
  }

}

